import numpy as np
import cv2
import math


def xx(x):
    if x > 0:
        return x - 180
    else:
        return x + 180


def angle(img, rects, landmarks):
    rets = []

    for i, rect in enumerate(rects):
        landmark = landmarks[i]
        size = img.shape

        image_points = np.array([(landmark[30].x, landmark[30].y),  # Nose tip
                                 (landmark[8].x, landmark[8].y),  # Chin
                                 (landmark[36].x, landmark[36].y),  # Left eye left corner
                                 (landmark[45].x, landmark[45].y),  # Right eye right corne
                                 (landmark[48].x, landmark[48].y),  # Left Mouth corner
                                 (landmark[54].x, landmark[54].y)  # Right mouth corner
                                 ], dtype="double")

        # 3D model points.
        model_points = np.array([(0.0, 0.0, 0.0),  # Nose tip
                                 (0.0, -330.0, -65.0),  # Chin
                                 (-225.0, 170.0, -135.0),  # Left eye left corner
                                 (225.0, 170.0, -135.0),  # Right eye right corne
                                 (-150.0, -150.0, -125.0),  # Left Mouth corner
                                 (150.0, -150.0, -125.0)  # Right mouth corner
                                 ])

        # Camera internals
        focal_length = size[1]
        center = (size[1] / 2, size[0] / 2)
        camera_matrix = np.array([[focal_length, 0, center[0]],
                                  [0, focal_length, center[1]],
                                  [0, 0, 1]], dtype="double")

        dist_coeffs = np.zeros((4, 1))  # Assuming no lens distortion
        (success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix,
                                                                      dist_coeffs)
        # print( "Camera Matrix :\n {0}".format(camera_matrix))
        # print( "Rotation Vector:\n {0}".format(rotation_vector))
        # print( "Translation Vector:\n {0}".format(translation_vector))

        # x down +
        # y clockwise +
        # z right +
        rets.append({
            'x': xx(math.degrees(rotation_vector[0])),
            'y': math.degrees(rotation_vector[1]),
            'z': math.degrees(rotation_vector[2])
        })

    return rets
